package model.route;

import java.util.ArrayList;
import model.Model;
import model.mapping.MapPoint;

public class AstarRoute {

	private Model model;
	private ArrayList<MapPoint> solutionPath;
	private ArrayList<AstarNode> open;
	private ArrayList<AstarNode> closed;
	private AstarNode solution;
	private MapPoint begin;
	private MapPoint end;
	
	public AstarRoute(Model model,MapPoint begin,MapPoint end) {
		
		this.model = model;
		this.begin = begin;
		this.end = end;
		open = new ArrayList<AstarNode>();
		closed = new ArrayList<AstarNode>();
	}

	public ArrayList<MapPoint> getSolutionPath() {
		return solutionPath;
	}

	public void setSolutionPath(ArrayList<MapPoint> solutionPath) {
		this.solutionPath = solutionPath;
	}

	public ArrayList<AstarNode> getOpen() {
		return open;
	}

	public void setOpen(ArrayList<AstarNode> open) {
		this.open = open;
	}

	public ArrayList<AstarNode> getClosed() {
		return closed;
	}

	public void setClosed(ArrayList<AstarNode> closed) {
		this.closed = closed;
	}

	public MapPoint getBegin() {
		return begin;
	}

	public void setBegin(MapPoint begin) {
		this.begin = begin;
	}

	public MapPoint getEnd() {
		return end;
	}

	public void setEnd(MapPoint end) {
		this.end = end;
	}

	public ArrayList<MapPoint> getAstarRoute() {
	
		AstarNode current = null;
		Boolean found = false;
		int h = Math.abs(begin.x - end.x) + Math.abs(begin.y - end.y);
		int g = 0;
		AstarNode beginning = new AstarNode(begin,g,h,null);
		open.add(beginning);
		while (!open.isEmpty() && !found) {
			current = getBestOpen();
			closed.add(current);
			if (current.getPoint().compareTo(end) == 1) {
				found = true;
				solution = current;
			}
			int x = current.getPoint().x;
			int y = current.getPoint().y;
			for (int i=x-1;i<=x+1&&!found;i++) {
				for (int j=y-1;j<=y+1&&!found;j++) {
					int agentDirection = model.getMapCheckouts().getDirection(current.getPoint().x,current.getPoint().y,i,j,false);
					if (model.getMapCheckouts().isInMap(i,j) && !model.getMapCheckouts().isSamePlace(x,y,i,j)) {
						MapPoint point = model.getModelMapBuilder().getDriverMap()[i][j];
						if (point.isRoad() && model.getMapCheckouts().isAceptableDirection(agentDirection,point)) {
							g = Math.abs(begin.x - point.x) + Math.abs(begin.y - point.y);
							h = Math.abs(point.x - end.x) + Math.abs(point.y - end.y);
							AstarNode candidate = new AstarNode(point,g,h,current);
							if (!model.getMapCheckouts().isRightLane(point))
								h = h + 10;
							if (!isInClosed(candidate) && !isInOpen(candidate)) 
								open.add(candidate);
							else 
								adjustNode(current,candidate);
						}
					}
				}
			}
		}
		if (found) {
			solutionPath = generateSolutionPath();
		}
		else {
			solution = getBestPath(closed,end);
			solutionPath = generateSolutionPath();
		}
		return solutionPath;
	}
	
	public AstarNode getBestOpen() {
		
		int pos = 0;
		AstarNode min = open.get(pos);
		for (int i=1;i<open.size();i++) {
			AstarNode candidate = open.get(i);
			if (candidate.compareTo(min) == -1) {
				pos = i;
				min = candidate;
			}
			else if (candidate.compareTo(min) == 0) {
				if (candidate.getH() < min.getH()) {
					pos = i;
					min = candidate;
				}
			}
		}	
		return open.remove(pos);
	}
	
	public Boolean isInClosed(AstarNode node) {
		
		Boolean found = false;
		for (int i=0;i<closed.size()&&!found;i++) {
			MapPoint closedPoint = closed.get(i).getPoint();
			MapPoint nodePoint = node.getPoint();
			if (closedPoint.compareTo(nodePoint) == 1) 
				found = true;
		}
		return found;
	}
	
	public Boolean isInOpen(AstarNode node) {
		
		Boolean found = false;
		for (int i=0;i<open.size()&&!found;i++) {
			MapPoint openPoint = open.get(i).getPoint();
			MapPoint nodePoint = node.getPoint();
			if (openPoint.compareTo(nodePoint) == 1)
				found = true;
		}
		return found;
	}
	
	public ArrayList<MapPoint> generateSolutionPath() {
		
		ArrayList<MapPoint> path = new ArrayList<MapPoint>();
		AstarNode auxiliary = solution;
		while (auxiliary != null) {
			path.add(0,auxiliary.getPoint());
            auxiliary = auxiliary.getFather();
        }
		path.remove(0);
        return path;
	}
	
	public void adjustNode(AstarNode current,AstarNode candidate) {
		
		int distance = Math.abs(current.getPoint().x - candidate.getPoint().x)
			+ Math.abs(current.getPoint().y - candidate.getPoint().y);
		int newG = current.getG() + distance;
		if (candidate.getG() > newG) {
			candidate.setG(newG);
			candidate.setF(candidate.getG() + candidate.getH());
			candidate.setFather(current);
		}
	}
	
	public AstarNode getBestPath(ArrayList<AstarNode> closed,MapPoint end) {
		
		AstarNode bestOption = closed.get(0);
		int bestDistance = bestOption.getH();
		for (int i=1;i<closed.size();i++) {
			int distance = closed.get(i).getH();
			if (distance < bestDistance) {
				bestDistance = distance;
				bestOption = closed.get(i);
			}
		}
		return bestOption;
	}
}